package net.nampham.visioncontrol.model;

import java.util.ArrayList;

/** For static obstacles */
public class TriangleMesh {
	/** Array of vertices */
	public Vec2[] vertices;
	
	/** Indices array: one element is a triangle with vertex index in vertices array */
	public TrippleInt[] indices;
	
	/** Element i has 3 values. Value j is a triangle adjacent to edge j of triangle i.*/
	TrippleInt[] adjacencies;
	
	public int triangle_num=0;
	
	public TriangleMesh() {
		vertices = new Vec2[8];
		vertices[0] = new Vec2(0,0);
		vertices[1] = new Vec2(3,0);
		vertices[2] = new Vec2(3,3);
		vertices[3] = new Vec2(0,3);

		vertices[4] = new Vec2(1,1);
		vertices[5] = new Vec2(2,1);
		vertices[6] = new Vec2(2,2);
		vertices[7] = new Vec2(1,2);
		

		//TODO: do triangulation
		triangle_num = 8;
		indices = new TrippleInt[triangle_num];
		indices[0] = new TrippleInt(0, 1, 4);
		indices[1] = new TrippleInt(4, 1, 5);

		indices[2] = new TrippleInt(1, 2, 5);
		indices[3] = new TrippleInt(5, 2, 6);

		indices[4] = new TrippleInt(2, 3, 6);
		indices[5] = new TrippleInt(7, 6, 3);

		indices[6] = new TrippleInt(0, 7, 3);
		indices[7] = new TrippleInt(0, 4, 7);


		//TODO: test this code
		//automatically fill in adjacency
		adjacencies = new TrippleInt[triangle_num];
		for (int idx1=0; idx1<triangle_num; idx1++){
			TrippleInt tri = indices[idx1];
			adjacencies[idx1] = new TrippleInt(-1, -1, -1);
			for (int edge=0;edge<3;edge++){
				int v0 = tri.val[edge];
				int v1 = tri.val[(edge+1)%3];
				boolean found = false;
				int idx2;
				for (idx2=0; idx2<triangle_num; idx2++){
					TrippleInt adj = indices[idx2];
					if (adj==tri) continue;
					for (int adj_edge=0;adj_edge<3;adj_edge++){
						int a_v0 = adj.val[adj_edge];
						if (a_v0==v1){
							if (adj.val[(adj_edge+1)%3]==v0){
								found=true;
								break;
							}
						}
					}
					if (found) break;
				}
				if (found){
					adjacencies[idx1].val[edge] = idx2;
				}
			}
		}
		int abc=0;
	}	

	public Vec2 GetTriVer(int tri_idx, int ver_idx){
		return vertices[indices[tri_idx].val[ver_idx]];
	}
	
	public Vec2[] FindPath(Vec2 start, Vec2 destination){
		return new Vec2[1];
	}
	
	public enum VisionType {
		FULL, HALF
	};
	public enum PointType{
		EXPANDED, UNEXPANDED_PENDING, UNEXPANDED_CONSUMED, VERTEX
	}
	class Point{
		PointType mType;
		int mEdge_id;
		int mVid;
		Vec2 mPoint;
		Ray2 mRay;
	}
	class Edge{
		VisionType mVType;		
		int tri_id;
		float mDistance;
		int mDepth;
		Point[] mPoints;	//index 0: right point, 1: left point
	}
		
	public void DebugDraw(){
		if (CONFIG._DEBUGGER!=null){
			for (int i=0; i<triangle_num;i++){
				for (int j=0; j<3; j++){
					Vec2 v0 = GetTriVer(i, j);
					Vec2 v1 = GetTriVer(i, (j+1)%3);
					CONFIG._DEBUGGER.renderLine(v0, v1, DColor.RED);
				}
			}
		}
	}
		
	public VisionField FindVision(Vec2 start, Angle angle){
		//perform search for every angle range
		//start with 1 angle range generated from angle input
		//1 angle range resides in a triangle
		//produce at most two angle ranges, out of two different triangles
		
		//some angle ranges should not compute intersection
		//(one of its side is at a triangle vertex)
		//unless it is the original angle side, it will be the vertex angle!
		//--> two types of angles
		//type 1: angle type --> intersect
		//type 2: vertex type --> determine it goes inward or outward the triangle!
		//- inward: intersect --> become type 1 on the next triangle!
		//  - joinable --> type 3!
		//    - create a joinable point (without computing the intersection point!)
		//    - if a joinable point is 'create' the second time (keys: vertex and triangle!)
		//      they will be joined (deleted), two angle ranges will become one!
		//    - if use breath first search, the points could be joined in different iterations!
		//      - could not determine whether a 'joinable' point is not 'joinable'!
		//      --> possible! it is not 'joinable' when the projecting vertex is 'consumed'
		//			(lied on an unpassable edge OR OUT OF DISTANCE edge)!
		//          - if it does not lie in an 'unpassable' edge, it will be joined later
		//- outward: generate next angle side for the adjacent triangle!
		//--> searching: 'joinable' range will be expanded last!
		//--> after join, the angle range could still be joinable!
		
		VisionField ret = new VisionField();
		
		
		//angle and triangle are in counter-clockwise
		Angle vision_range = Angle.FromDegree(60);
		Angle l_angle = angle.add(vision_range.mul(0.5));
		Angle r_angle = angle.dec(vision_range.mul(0.5));
		
		Ray2[] rays = new Ray2[2];
		rays[0] = new Ray2(start, r_angle);
		rays[1] = new Ray2(start, l_angle);
		
		
		int cur_triangle = 0;
		//prepare the first two edges
		ArrayList<Edge> equeue = new ArrayList<Edge>();
		int edge_id = -1;
		for (int r=0;r<2;r++){
			for (int e=0;e<3;e++){
				Vec2[] vs = new Vec2[2];
				vs[0] = GetTriVer(cur_triangle, e);
				vs[1] = GetTriVer(cur_triangle, (e+1)%3);
				
				Vec2 li = rays[r].Intersect(new Segment2(vs[0], vs[1]));
				if (li!=null){
					if (li.equals(vs[1-r])){
						continue;//it should lie on the next edge
					}
					Point p0 = new Point();
					p0.mEdge_id = e;
					if (li.equals(vs[r])){
						p0.mType = PointType.VERTEX;
						p0.mVid = r;
					}else{
						p0.mType = PointType.EXPANDED;
						p0.mPoint = li;
						p0.mRay = rays[r];						
					}
					
					if (edge_id==e){//old edge intersect two times
						Edge oldE = equeue.get(0);
						oldE.mPoints[1]=p0;//left point
					}else{				
						Edge newE = new Edge();
						newE.tri_id = cur_triangle;
						newE.mPoints = new Point[2];
						
						Point p1 = new Point();
						p1.mType = PointType.VERTEX;
						p1.mEdge_id = e;
						p1.mVid = r;	//the left ray get the right vertex and vice versa
						
						newE.mPoints[0] = p0;
						newE.mPoints[1] = p1;
						equeue.add(newE);
						edge_id = e;
					}
					break;//done with this ray!
				}
			}
		}
		
		//*render equeue
		if (CONFIG._DEBUGGER != null){
			for (Edge e : equeue){
				Vec2 v0 = start;
				Vec2 v1;
				for (Point p : e.mPoints){
					if (p.mType == PointType.EXPANDED){
						v1 = p.mPoint;
					}else{
						v1 = this.vertices[
						        this.indices[e.tri_id]
						            .val[(p.mEdge_id+p.mVid)%3]
						     ];
					}
					CONFIG._DEBUGGER.renderLine(v0, v1, DColor.BLUE);
				}
			}
		}
		//*/
		
		//*render rays
		if (CONFIG._DEBUGGER!=null){
			//CONFIG._DEBUGGER.renderLine(rays[0].origin, rays[0].dest, DColor.WHITE);
			//CONFIG._DEBUGGER.renderLine(rays[1].origin, rays[1].dest, DColor.GREEN);
			CONFIG._DEBUGGER.renderTriangle(rays[0].origin, rays[0].dest, rays[1].dest, DColor.GREEN);
		}
		//*/

		/*
		//loop the edges
		while (true){
			//find the next edge
			Edge curE = equeue.remove(0);
			for (int p=0;p<2;p++){
				Point cP = curE.mPoints[p];
				Edge newE = new Edge();
				newE.tri_id = this.adjacencies[curE.tri_id].val[cP.mEdge_id];
				
				int newEdgeId=-1;
				for (newEdgeId=0; newEdgeId<3; newEdgeId++){
					if (this.adjacencies[newE.tri_id].val[newEdgeId]==curE.tri_id){
						break;
					}
				}
				if (cP.mType==PointType.EXPANDED){
					Point nP = new Point();
					for (int checkEdge=0;checkEdge<3; checkEdge++){
						if (checkEdge!=newEdgeId){
							Vec2 v0 = GetTriVer(newE.tri_id, checkEdge);
							Vec2 v1 = GetTriVer(newE.tri_id, (checkEdge+1)%3);
						}
					}
				}
			}
			
		}
		//*/
		
		/*
		edge_queue = search(langle, rangle, start, triangle);
		{
			return 1 to 2 edges
		}
		next_edge = edge_queue[0];
		{
			edges[] = find_edges(next_edge);
			{
				local new_type
				if point.type is 'computed' | 'uncomputed_consumed'
					next_point.type = 'computed'
				else if point.type is 'vertex' {
					if (check_outside){
						next_vertex.type = vertex
					}else
					{
						next_point.type = 'uncomputed_pending'
					}
				}
				if edge_id is 'unpassable'
				{
					'consume' the point
				}else if edge_id is 'half_passable'
				{
					vision_type = 'half'
				}
				point.joinable_type = will_be_never
			}
			edge_queue.add(edges);
			edges
			{
				2 point, point left and point right based on 'array index'
				{
					type: computed, uncomputed_pending, uncomputed_consumed, vertex
					edge_id
					val: (float)
						computed: 
							abs: on edge_id OR angle
						uncomputed:
							ori_vertex_id
						vertex: 0 or 1
				}
				vision type: full or half
				distance: the distance of the nearer point!
			}
			edge_queue.join();
			{
				foreach 'uncomputed_pending' points
					find 'its other half': same edge_id and ori_vertex_id
						if 'this half' is 'consumed'
							set both to 'consumed'
						else
							joint two edges to one
			}
			next_edge = edge_queue.get_next();
			{
				priority:
					-lowest depth! (or distance?)
					-vertex edge
					-outermost edge
					-joinable edge
			}
		}
		//*/
		
		return ret;
	}
}
